#define USESYPDLL
#include <Syp/SypEngine.h>
#include <iostream>
#include <dae.h>
#include <dae/daeSIDResolver.h>
#include <dom/domCOLLADA.h>
#include <dom/domConstants.h>
#include <NvTriStrip.h>
using namespace std;
using namespace Syp;

class Vertex
{
public:
	bool operator ==(const Vertex& v)
	{
		if(pos == v.pos && nor == v.nor && tex == v.tex)
		{
			return true;
		}
		return false;
	};
	Point3 pos;
	Point3 nor;
	Point3 tex;
};

class oldLimits
{
public:
	PInt indexStart;
	PInt indexEnd;
};
bool runGame = true;
void onClose()
{
	runGame = false;	
}

template<class T>
void takeLarger(T val, T& radius)
{
	if(val<0)
	{
		val = -val;
	}
	if(val > radius)
	{
		radius = val;
	}
}

String verticesID;
String normalsID;
String texcoordsID;
//Data
FloatArray vertices;
FloatArray normals;
FloatArray texcoords;
FloatArray unknown;
PIntArray indices;

std::vector<oldLimits> LimitsArray;
std::map<PInt,PInt> oldToNewIndex;

PInt numSemantics;
PInt vOffset;
PInt nOffset;
PInt tOffset;

PInt numOptimized = 0;

std::vector<Vertex> VertexArray;
std::vector<Vertex> FinalVertexArray;
std::vector<Vertex>::iterator vIt;

PShortArray indicesNew;

Syp::IndexedMesh* mesh = 0;
String meshName;
Syp::Material* m = 0;
Quat MaxToGL;

DAE* myDae;

void processAnimation()
{	
	//Maps Bone names to their numerical id for a skeleton
	std::map<String,PShort> BoneNameMapping;
	//Used in conjunction with WeightsArray to form a VWeight
	std::vector<String> BoneNameArray;
	FloatArray WeightsArray;
	PInt JointOff,WeightOff;
	PIntArray vCount, vIndex;


	PInt num0 = myDae->getDatabase()->getElementCount(0, COLLADA_ELEMENT_CONTROLLER, 0);
	cout<<"Number of Animated Controllers found : "<<num0<<"\n";
	domController* cont = 0;
	///For every skeleton
	for(PInt i=0;i<num0;++i)
	{
		std::map<short,short> parentMap;
		Skeleton* skeleton = Skeleton::createSkeleton(meshName + ".syps");
		String d;
		PInt error = myDae->getDatabase()->getElement((daeElement**)&cont, 0, 0, COLLADA_ELEMENT_CONTROLLER);
		cout<<" Error Code "<<i<<" : "<<error<<endl;
		d = cont->getId();
		cout<<"Controller ID : "<<d<<"\n";
		d = cont->getSkin()->getSource().getID();
		cout<<"Skin source ID : "<<d<<"\n";

		if(d != meshName)
		{
			cout<<"Skin source ID does not match current mesh, skipping "<<d<<"\n";
			continue;
		}
		else
		{
			mesh->setHasSkin(true);
		}

		//Get bind Shape matrix
		float bindShape[16];
		//	float FbindShape[16];

		cout<<"Bind shape matrix\n";

		for(int j=0;j<16;++j)
		{
			bindShape[j] = cont->getSkin()->getBind_shape_matrix()->getValue().get(j);
			cout<<bindShape[j]<<" ";
		}
		Math::TransposeMatrix(bindShape);

		//Transpose the matrix cos values are in row-major order.

		//	Math::MaxToGLMatrix(bindShape,FbindShape);

		/////////////////////////////////////////////////////////Inverse FflipYZ Matrix creation////////////////////////////////////
		//		float flipYZ[16];
		//	
		//		float FflipYZ[16];
		//	//	-- computes the matrix
		//	//axis1 = point3 1 0 0 0 ;
		//	//axis2 = point3 0 0 1 0;
		//	//axis3 = point3 0 -1 0 0;
		//	//t =	  point3 0 0 0 1;
		//
		//		Math::IdentityMatrix(flipYZ);
		//		flipYZ[5] = 0;
		//		flipYZ[6] = -1.0f;
		//		flipYZ[9] = 1.0f;
		//		flipYZ[10] = 0.0f;
		//		///Matrix from maxscript is row major
		//		Math::TransposeMatrix(flipYZ);
		//
		//        Math::InverseMatrix(flipYZ,FflipYZ);
		///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////		
		//		//-- multiplies by the inverse
		//		//Tform = Tform*inverse(m) ;
		//			float tempMat[16];
		//		Math::multMat(FflipYZ,bindShape, FbindShape);
		cout<<"\nFinal bindShape matrix after flip YZ \n";
		//HACK
		//FbindShape[12] = -FbindShape[12];
		//FbindShape[14] = -FbindShape[14];
		//DEBUG CODE
		float p[3];
		float r[3];
		r[0] = r[1] = r[2] = 0;
		p[0] = 10.0f;
		p[1] = 20.0f;
		p[2] = 30.0f;

		Math::MultVectorByMatrix(bindShape,p,1.0f,r);
		//DEBUG

		cout<<" Results are "<<r[0]<<" "<<r[1]<<" "<<r[2]<<"\n";
		/*for(int j=0;j<16;++j)
		{			
		cout<<FbindShape[j]<<" ";
		}*/

		mesh->setBindPoseMatrix(bindShape);


		size_t Cnt;
		FloatArray bindMatrix;
		////<joints>
		Cnt = cont->getSkin()->getJoints()->getInput_array().getCount();
		for(size_t j=0;j<Cnt;++j)
		{			
			domInputLocalRef dil = cont->getSkin()->getJoints()->getInput_array().get(j);
			d = dil->getSemantic();
			if(d == "INV_BIND_MATRIX")
			{
				d = dil->getSource().getID();
				daeElement* BA = 0;
				myDae->getDatabase()->getElement((daeElement**)&BA, 0, d.c_str(), 0);
				d = BA->getTypeName();
				if(d == COLLADA_ELEMENT_SOURCE)
				{
					domSource* ds = (domSource*)BA;
					cout<<"\nBind shape matrix count"<<ds->getFloat_array()->getCount()<<"\n";

					for(size_t k = 0;k<ds->getFloat_array()->getCount();++k)
					{
						bindMatrix.push_back(ds->getFloat_array()->getValue().get(k));
						cout<<bindMatrix[ bindMatrix.size()-1 ]<<" ";


					}
					Math::TransposeMatrix( &bindMatrix[0] );
				}

			}	




		}




		//<vertex_weights>
		Cnt = cont->getSkin()->getVertex_weights()->getInput_array().getCount();//num semantics
		cout<<"Joint has "<<static_cast<PInt>(Cnt)<<" input semantics\n";

		///Skin information
		for(size_t j=0;j<Cnt;++j)
		{//for every semantic
			domInputLocalOffsetRef dil = cont->getSkin()->getVertex_weights()->getInput_array().get(j);
			d = dil->getSemantic();
			cout<<d<<"\n";
			///Bone Array
			if(d == "JOINT")
			{
				JointOff = dil->getOffset();
				d = dil->getSource().getID();
				daeElement* BA = 0;
				myDae->getDatabase()->getElement((daeElement**)&BA, 0, d.c_str(), 0);
				d = BA->getTypeName();
				if(d == COLLADA_ELEMENT_SOURCE)
				{
					domSource* ds = (domSource*)BA;
					for(size_t k = 0;k<ds->getName_array()->getCount();++k)
					{						
						d = ds->getName_array()->getValue().get(k);
						BoneNameArray.push_back(d);
						parentMap[ static_cast<short>(BoneNameArray.size()-1)] = -1;
						BoneNameMapping[d] = BoneNameArray.size()-1;
						cout<<d<<" ";
					}
					cout<<"\n";

				}

			}
			///Weights Array
			if(d == "WEIGHT")
			{
				WeightOff = dil->getOffset();
				d = dil->getSource().getID();
				daeElement* BA = 0;
				myDae->getDatabase()->getElement((daeElement**)&BA, 0, d.c_str(), 0);
				d = BA->getTypeName();
				if(d == COLLADA_ELEMENT_SOURCE)
				{
					domSource* ds = (domSource*)BA;
					for(size_t k = 0;k<ds->getFloat_array()->getCount();++k)
					{
						float val = ds->getFloat_array()->getValue().get(k);
						WeightsArray.push_back(val);
						cout<<val<<" ";
					}
					cout<<"\n";

				}
			}
		}
		//vCount array
		size_t dsa = cont->getSkin()->getVertex_weights()->getVcount()->getValue().getCount();
		for(size_t o=0 ; o<dsa ; ++o)
		{
			PInt uio = cont->getSkin()->getVertex_weights()->getVcount()->getValue().get(o);
			cout<<" "<<uio;
			vCount.push_back(uio);

		}
		cout<<"\n";
		//vIndex Array
		dsa = cont->getSkin()->getVertex_weights()->getV()->getValue().getCount();
		for(size_t o=0 ; o<dsa ; ++o)
		{
			PInt uio = cont->getSkin()->getVertex_weights()->getV()->getValue().get(o);
			cout<<" "<<uio;
			vIndex.push_back(uio);

		}

		cout<<"vCount size is "<<vCount.size()<<"\n";
		cout<<"VWeight Array size is "<<WeightsArray.size()<<"\n";
		cout<<"BoneArray size is "<<BoneNameArray.size()<<"\n";
		PInt vCounter = 0;
		for(PInt zxc=0;zxc<vCount.size();zxc++)
		{//for every unique vertex by position
			Syp::VertexWeightSet vw;
			for(PInt b=0;b<vCount[zxc];++b)
			{				
				//ASSUMING THERE WILL ONLY EVER BE 2 INPUT SEMANTICS FOR VWEIGHTS
				vw.BoneID.push_back( BoneNameMapping[ 
					BoneNameArray[
						vIndex[vCounter + JointOff]
					] 
				]);
				vw.weight.push_back(WeightsArray[
					vIndex[
						vCounter + WeightOff]
				]);


				cout<<" "<<BoneNameArray[vIndex[vCounter + JointOff]] <<" "<<WeightsArray[vIndex[vCounter + WeightOff]				];
				vCounter+=Cnt;
			}
			mesh->getVWeightsRef().push_back(vw);
			cout<<"\n";
		}

		cout<<"Total VWeights in skin : "<<mesh->getVWeightsRef().size()<<"\n";

		////////////////////////////////////////////////Skeleton file building////////////////////////////////////////

		for(PInt sfb=0;sfb<BoneNameArray.size();++sfb)
		{
			String e;
			e = "./" + BoneNameArray[sfb];
			domNode* dn;
			//Try to get the Node with the same name
			domElement* tempEle = 0;
			myDae->getDatabase()->getElement((daeElement**)&dn, 0, 0, "visual_scene");
			daeSIDResolver resolver(dn, e.c_str());
			//
			dn = daeSafeCast<domNode>(resolver.getElement());
			Bone* bumbleBone = new Bone();
			Bone& b = *bumbleBone;
			b.ID = static_cast<short>(sfb);

			e = dn->getID();
			cout<<"\n\n\nNow processing : "<<e<<"\n";
			b.name = e;
			PInt similj = dn->getTranslate_array().getCount();
			float x = dn->getTranslate_array().get(0)->getValue().get(0);
			float y = dn->getTranslate_array().get(0)->getValue().get(1);
			float z = dn->getTranslate_array().get(0)->getValue().get(2);
			b.setPosition(x,y,z);
			Quat Final;
			Quat q2[4];
			for(PInt oiu = 0;oiu<dn->getRotate_array().getCount();++oiu)
			{
				Quat* q1;
				String haiz = dn->getRotate_array().get(oiu)->getSid();
				//YZ SWAP               q2 = Y Z X Others
				/*	if(haiz == "RotZ" )
				{
				q1 = &q2[0];
				}
				else if(haiz == "RotY" )
				{
				q1 = &q2[1];
				}
				else if(haiz == "RotX" )
				{
				q1 = &q2[2];
				}
				else
				{
				q1 = &q2[3];
				}*/
				q1 = &q2[oiu];
				//YZ SWAP				
				q1->x = dn->getRotate_array().get(oiu)->getValue().get(0);
				q1->y = dn->getRotate_array().get(oiu)->getValue().get(1);
				q1->z = dn->getRotate_array().get(oiu)->getValue().get(2);
				q1->w = dn->getRotate_array().get(oiu)->getValue().get(3);
				//q1->w = q1->w * (PI/180.0f);//change to radians becos values are in degrees
				*q1 = q1->convertToQuat();


				q1->Normalize();				

			}
			// Y        Z       X
			Final =  (q2[0] * q2[1]) * q2[2];
			Final.Normalize();

			b.setOrientation(Final); 

			daeTArray< daeSmartRef<daeElement> > children;
			//Retrieve children
			dn->getChildren(children);	
			
			cout<<"Bone has "<<children.getCount()<<" children \n";
			for(size_t st=0;st<children.getCount();++st  )
			{
				String e = children.get(st)->getTypeName();
				cout<<"Child : "<<e<<"\n";
				if(e == "node")
				{
					domNode* don2 = daeSafeCast<domNode>(children.get(st));
					e = don2->getSid();
						

					parentMap[ static_cast<short>(BoneNameMapping[e]) ] = b.ID;
					cout<<"Child name : "<<e<<" and has a parent with ID : "<<parentMap[static_cast<short>(BoneNameMapping[e])]<<"\n";
				}
			}

			//Bone collada inv matrix
			for(int q = 0; q < 16 ; ++q)
			{
				b.inv2[q] = bindMatrix[q];
			}



			cout<<"Bone Pos : "<< b.getPosition().x<<" "<<b.getPosition().y<<" "<<b.getPosition().z<<"\n";
			cout<<"Bone Rot in Quats : "<< b.getOrientation().x<<" "<<b.getOrientation().y<<" "<<b.getOrientation().z<<" "<<b.getOrientation().w<<"\n";
			///////////////////////////////Retrieve Bone Animation///////////////////////////////////
			PInt numAnimation = 0;
			numAnimation = myDae->getDatabase()->getElementCount(0, COLLADA_ELEMENT_ANIMATION, 0);
			cout<<"Number of <animation> tags found : "<<numAnimation<<"\n";
			for(PInt rr=0;rr<numAnimation;++rr)
			{
				//cout<<"--------------------------------------------\n";
				domAnimation* domAni = 0;

				myDae->getDatabase()->getElement((daeElement**)&domAni, rr, 0, COLLADA_ELEMENT_ANIMATION);
				PInt numChannels = domAni->getChannel_array().getCount();
				for(PInt nn=0;nn<numChannels;++nn)
				{
					String f = domAni->getChannel_array().get(nn)->getTarget();
					String Target = f.substr(0,f.find_first_of('/'));
					String pTgt;
					//cout<<"Target is : "<<Target<<"\n";
					if(Target != b.name)
					{
						//	cout<<"Channel does not target "<<b.name<<", Skipping....\n";
						break;
					}
					pTgt = f.substr(f.find_first_of('/')+1, (f.find_first_of('.') - f.find_first_of('/')-1) );
					cout<<"Parameter being Targetted : "<<pTgt<<"\n";
					if(pTgt == "translation" || pTgt == "RotX" || pTgt == "RotY" || pTgt == "RotZ")
					{


						domSampler* dsou;
						String sourceId;
						sourceId = (domAni->getChannel_array().get(nn)->getSource().getID());
						myDae->getDatabase()->getElement((daeElement**)&dsou, 0, sourceId.c_str(), COLLADA_ELEMENT_SAMPLER);
						cout<<"Channel points to "<<dsou->getId();
						size_t inptCnt = dsou->getInput_array().getCount();
						for(size_t st = 0;st<inptCnt;++st)
						{
							domSource* dso2;
							myDae->getDatabase()->getElement((daeElement**)&dso2, 0, 
								dsou->getInput_array().get(st)->getSource().getID()
								, COLLADA_ELEMENT_SOURCE);	
							size_t keyFrameCount = 0;
							String ng = dsou->getInput_array().get(st)->getSemantic();
							if(ng=="INPUT")
							{															
								keyFrameCount = dso2->getFloat_array()->getCount();
								if(keyFrameCount > b.KFrame.size())
								{
									b.KFrame.resize(keyFrameCount);
								}
								for(size_t argh=0;argh<keyFrameCount;++argh)
								{
									b.KFrame[argh].time = dso2->getFloat_array()->getValue().get(argh);

								}

							}
							else if(ng=="OUTPUT")
							{								
								keyFrameCount = dso2->getFloat_array()->getCount();
								/*if(keyFrameCount > b.KFrame.size())
								{
								b.KFrame.resize(keyFrameCount);
								}*/
								if(pTgt == "translation")
								{
									if(f[(f.size()-1)] == 'X')
									{
										for(size_t argh=0;argh<keyFrameCount;++argh)
										{
											b.KFrame[argh].Pos.x = dso2->getFloat_array()->getValue().get(argh);

										}

									}
									else if(f[(f.size()-1)] == 'Y')
									{//YZ SWAP
										for(size_t argh=0;argh<keyFrameCount;++argh)
										{
											b.KFrame[argh].Pos.y = dso2->getFloat_array()->getValue().get(argh);

										}

									}
									else if(f[(f.size()-1)] == 'Z')
									{//YZ SWAP
										for(size_t argh=0;argh<keyFrameCount;++argh)
										{
											b.KFrame[argh].Pos.z = dso2->getFloat_array()->getValue().get(argh);

										}

									}
									else//it is new collada type where XYZ are all in 1 array
									{
										for(size_t argh=0;argh<keyFrameCount/3;argh++)
										{
											b.KFrame[argh].Pos.x = dso2->getFloat_array()->getValue().get(argh * 3);
											b.KFrame[argh].Pos.y = dso2->getFloat_array()->getValue().get(argh * 3 + 1);
											b.KFrame[argh].Pos.z = dso2->getFloat_array()->getValue().get(argh * 3 + 2);
										}
									}
								}
								else if(pTgt == "RotX")
								{

									for(size_t argh=0;argh<keyFrameCount;++argh)
									{
										b.KFrame[argh].Rot.x = dso2->getFloat_array()->getValue().get(argh);

									}


								}
								else if(pTgt == "RotY")
								{

									for(size_t argh=0;argh<keyFrameCount;++argh)
									{//YZ SWAP
										b.KFrame[argh].Rot.y = dso2->getFloat_array()->getValue().get(argh);

									}


								}
								else if(pTgt == "RotZ")
								{

									for(size_t argh=0;argh<keyFrameCount;++argh)
									{
										b.KFrame[argh].Rot.z = dso2->getFloat_array()->getValue().get(argh);

									}


								}



							}
						}

					}

				}

			}




			for(size_t nmn=0;nmn < b.KFrame.size();++nmn)
			{
				cout<<"KeyFrame "<<nmn<<" output:\n";
				cout<<"Time : "<<b.KFrame[nmn].time<<"\n";
				cout<<"Pos : "<<b.KFrame[nmn].Pos.x<<" "<<b.KFrame[nmn].Pos.y<<" "<<b.KFrame[nmn].Pos.z<<"\n";
				cout<<"Rot : "<<b.KFrame[nmn].Rot.x<<" "<<b.KFrame[nmn].Rot.y<<" "<<b.KFrame[nmn].Rot.z<<" "<<b.KFrame[nmn].Rot.w<<"\n";

				AngleAxis oldX;
				oldX.x = 1.0f;
				oldX.w = b.KFrame[nmn].Rot.x;

				AngleAxis oldY;
				oldY.y = 1.0f;
				oldY.w = b.KFrame[nmn].Rot.y;

				AngleAxis oldZ;
				oldZ.z = 1.0f;
				oldZ.w = b.KFrame[nmn].Rot.z;

				oldX = oldX.convertToQuat();
				oldX.Normalize();
				oldY = oldY.convertToQuat();
				oldY.Normalize();
				oldZ = oldZ.convertToQuat();
				oldZ.Normalize();

				Quat FinalRotation = (oldZ * oldY)  * oldX;
				//	FinalRotation = MaxToGL * FinalRotation;
				FinalRotation.Normalize();
				b.KFrame[nmn].Rot = FinalRotation;
				Quat omg = FinalRotation.convertToAngleAxis();

				cout<<"Converted Rot : "<<b.KFrame[nmn].Rot.x<<" "<<b.KFrame[nmn].Rot.y<<" "<<b.KFrame[nmn].Rot.z<<" "<<b.KFrame[nmn].Rot.w<<"\n";


				/*FinalRotation = FinalRotation.convertToAngleAxis();
				cout<<"OLDRot : "<<FinalRotation.x<<" "<<FinalRotation.y<<" "<<FinalRotation.z<<" "<<FinalRotation.w<<"\n";*/

			}

			skeleton->getBonesRef().push_back(bumbleBone);

		}

		//We have to link up the Bones with their parents.
		for(short b=0;b<skeleton->getNumBones();++b)
		{
			short parentID = parentMap[b];

			Bone& babo = skeleton->getBoneRef(b);
			//Quat orig = babo.getOrientation();
			//orig = orig.convertToAngleAxis();
			//Quat newQ = orig;
			////Quat bindPose;
			///*Quat newOrig = orig * MaxToGL;
			//newOrig.Normalize();
			//babo.setOrientation(newOrig);*/

			//cout<<"Orig node rot : "<<orig.x<<" "<<orig.y<<" "<<orig.z<<" "<<orig.w<<"\n";
			//newQ.x = -orig.x;
			//newQ.y = -orig.z;
			//newQ.z = orig.y;
			//cout<<"NewQ node rot : "<<newQ.x<<" "<<newQ.y<<" "<<newQ.z<<" "<<newQ.w<<"\n";
			//newQ.w = newQ.w * (PI/180.0f);
			//newQ = newQ.convertToQuat();
			//newQ.Normalize();
			/*bindPose.x = -newQ.x;
			bindPose.y = -newQ.y;
			bindPose.z = -newQ.z;
			bindPose.w = newQ.w;*/
			//babo.setOrientation(newQ);
			cout<<"Final node rot : "<<babo.getOrientation().x<<" "<<babo.getOrientation().y<<" "<<babo.getOrientation().z<<" "<<babo.getOrientation().w<<"\n\n\n";

			for(size_t nmn=0;nmn < babo.KFrame.size();++nmn)
			{
				//babo.KFrame[nmn].Rot = MaxToGL * babo.KFrame[nmn].Rot;
				/* Commenting 4/6/2007
				orig = babo.KFrame[nmn].Rot;
				orig = orig.convertToAngleAxis();
				//orig.Normalize();
				newQ = orig;
				//Quat newQ = orig * MaxToGL;
				cout<<"Orig node frame "<<nmn<<" : "<<orig.x<<" "<<orig.y<<" "<<orig.z<<" "<<orig.w<<"\n";
				newQ.x = orig.x;
				newQ.y = orig.y;
				newQ.z = orig.z;
				cout<<"NewQ node rot : "<<newQ.x<<" "<<newQ.y<<" "<<newQ.z<<" "<<newQ.w<<"\n";
				newQ.w = newQ.w * (PI/180.0f);
				newQ = newQ.convertToQuat();
				newQ.Normalize();
				//We take away the initial bind rotation of the bone away.
				//newQ = bindPose * newQ;
				//babo.KFrame[nmn].Rot = newQ;*/
				cout<<"Final node rot : "<<babo.KFrame[nmn].Rot.x<<" "<<babo.KFrame[nmn].Rot.y<<" "<<babo.KFrame[nmn].Rot.z<<" "<<babo.KFrame[nmn].Rot.w<<"\n";

			}
			if(parentID == -1)
			{
				//no parent
			}
			else
			{
				//parentmap gets the bones parent id, getBoneRef returns the 
				skeleton->getBoneRef(b).bindToNode( &skeleton->getBoneRef(parentID) );

			}



		}


		skeleton->writeToFile(meshName + ".syps");
	}
}

void processData()
{
	for(PInt eh=0;eh<LimitsArray.size();++eh)
	{
		IndexBlock ib;

		ib.offset = indicesNew.size();

		/*cout<<"Limits Array index start : "<<LimitsArray[eh].indexStart<<"\n";
		cout<<"Limits Array index end : "<<LimitsArray[eh].indexEnd<<"\n";
		cout<<"numSemantics : "<<numSemantics<<"\n";*/

		PInt iOffset = vOffset;
		takeLarger(nOffset,iOffset);
		takeLarger(tOffset,iOffset);
		++iOffset;


		for(PInt i=LimitsArray[eh].indexStart;i<LimitsArray[eh].indexEnd;i+=iOffset)
		{
			//cout<<"i is : "<<i<<"\n";
			//Construct the vertex
			Vertex v;
			v.pos.x = vertices[ indices[ i + vOffset ] * 3 ];
			v.pos.y = vertices[ indices[ i + vOffset ] * 3 + 1 ];
			v.pos.z = vertices[ indices[ i + vOffset ] * 3 + 2 ];
			//cout<<"Position "<<v.pos.x<<" "<<v.pos.y<<" "<<v.pos.z<<"\n";

			v.nor.x = normals[ indices[ i + nOffset ] * 3];
			v.nor.y = normals[ indices[ i + nOffset ] * 3 + 1 ];
			v.nor.z = normals[ indices[ i + nOffset ] * 3 + 2 ];	
			v.nor.Normalize();
			//cout<<"Normal "<<v.nor.x<<" "<<v.nor.y<<" "<<v.nor.z<<"\n";
			if(texcoordsID != "")
			{
				v.tex.x = texcoords[ indices[ i + tOffset ] * 3];
				v.tex.y = texcoords[ indices[ i + tOffset ] * 3 + 1 ];
				v.tex.z = texcoords[ indices[ i + tOffset ] * 3 + 2 ];
			}

			vIt = std::find(VertexArray.begin(),VertexArray.end(),v);

			if(vIt != VertexArray.end())
			{//we have a similar vertex already
				PInt x = static_cast<PInt>(distance(VertexArray.begin(),vIt));
				indicesNew.push_back(x);				
				numOptimized++;

			}
			else
			{
				VertexArray.push_back(v);
				mesh->getDataRef().push_back(v.pos.x);
				mesh->getDataRef().push_back(v.pos.y);
				mesh->getDataRef().push_back(v.pos.z);

				mesh->getDataRef().push_back(v.nor.x);
				mesh->getDataRef().push_back(v.nor.y);
				mesh->getDataRef().push_back(v.nor.z);

				if(texcoordsID != "")
				{
					mesh->getDataRef().push_back(v.tex.x);
					mesh->getDataRef().push_back(v.tex.y);				
				}

				indicesNew.push_back(static_cast<PShort>(VertexArray.size()-1));
			}
			oldToNewIndex[indicesNew.size()-1] = indices[ i + vOffset ];

		}	


		float radiusX = 0.0f;
		float radiusY = 0.0f;
		float radiusZ = 0.0f;
		//Generate Radius for Frustum culling, this radius is 360Degrees, it encompasses all the axes.
		for(size_t i=0;i<VertexArray.size();++i)
		{			
			takeLarger(VertexArray[i].pos.x,radiusX);
			takeLarger(VertexArray[i].pos.y,radiusY);
			takeLarger(VertexArray[i].pos.z,radiusZ);

		}

		mesh->setBoundingRadius(  sqrt( pow(radiusX,2) + pow(radiusY,2) + pow(radiusZ, 2)) );


		PShortArray::iterator tempIT = indicesNew.begin();
		advance(tempIT,ib.offset);


		ib.count = indicesNew.size() - ib.offset;
		ib.materialID = eh;

		ib.smallest = (*std::min_element(tempIT ,indicesNew.end()));
		ib.largest = (*std::max_element(tempIT ,indicesNew.end()));
		cout<<"\nSmallest index was "<<ib.smallest<<"\n";
		cout<<"Largest index was "<<ib.largest<<"\n";	

		//Re-ordering of indices optimisation here, FOR EVERY SUBMESH/DRAWCALL
		PrimitiveGroup* pg;
		SetListsOnly(true);
		SetCacheSize(14);
		PShort numGrps = 0;
		GenerateStrips(&indicesNew[ ib.offset ], ib.count , &pg, &numGrps);
		//indicesNew.clear();
		for( int i = 0; i < pg->numIndices; ++i)
		{
			indicesNew[ ib.offset + i ] = pg->indices[i];
		}
		
		delete[] pg;
		/////////////////////////////////////////////

		mesh->getDrawCallsRef().push_back(ib);		

	}
	if(texcoordsID != "")
	{
		mesh->setVertexType(VNT);
	}
	else
	{
		mesh->setVertexType(VN);
	}
	cout<<"\n\nNew Indices array has a count of "<<static_cast<PInt>(indicesNew.size())<<"\n";
	//cout<<"NewIndices to VWeight mapping has a size of "<<oldToNewIndex.size()<<"\n";
	bool foundMatch;
	for(PInt qwe=0;qwe<VertexArray.size();++qwe)
	{//for every vertex in our mesh's data array
		Vertex& theV = VertexArray[qwe];
		for(PInt similj=0;similj<vertices.size();similj+=3)
		{//go through every vertex in Collada's positional vertex array
			foundMatch = false;
			if(theV.pos.x == vertices[similj] && theV.pos.y == vertices[similj + 1] && theV.pos.z == vertices[similj + 2])
			{//pushback the ID of the vweight to use
				mesh->getSkinIDRef().push_back(static_cast<PShort>( similj/3 ));
				foundMatch = true;
				break;
			}		

		}	
		if(foundMatch == false)
		{
			cout<<"This could be a problem.\n";
		}
		//cout<<" "<<oldToNewIndex[qwe];
	}
	cout<<"Mesh has "<<mesh->getSkinIDRef().size()<<" SkinIDs\n";
	mesh->getIndexArrayRef().insert(mesh->getIndexArrayRef().begin(),indicesNew.begin(),indicesNew.end());
	cout<<"IndexedMesh has successfully copied indices and has a count of : "<<mesh->getIndexArrayRef().size()<<"\n";
	for(PInt i=0;i<indicesNew.size();++i)
	{
		//cout<<indicesNew[i]<<" ";
	}
	processAnimation();




	indicesNew.clear();
	vertices.clear();
	normals.clear();
	texcoords.clear();
	unknown.clear();
	indices.clear();
}

int main()
{

	//MaxToGL.x = -0.707107f;
	//MaxToGL.y = 0;
	//MaxToGL.z = 0;
	//MaxToGL.w = 0.707107;
	MaxToGL.x = 1.0f;
	MaxToGL.y = 0;
	MaxToGL.z = 0;
	MaxToGL.w = -90.0f * (PI/180.0f);
	MaxToGL = MaxToGL.convertToQuat();


	Syp::SypEngine::createSypEngine();
	SypEngine& g = SypEngine::getSypEngine();
	g.init("Converter",1024,768,false);
	g.setWindowCloseCallback(onClose);


	Syp::SceneManager& SM = SceneManager::getSceneManager();
	Camera* myCam = Camera::createCamera();	
	myCam->setPosition(0.0f,0.0f,-10.0f);
	SM.setActiveCamera(myCam);


	PInt num = 0;
	PInt numGeo = 0;
	PInt offset = 0;

	myDae = new DAE;
	int error = myDae->load("export.dae");
	//myDae->getDatabase()->getCollection((daeUInt)(0))->setModified(true);
	cout<<" Error Code : "<<error<<endl;

	numGeo = myDae->getDatabase()->getElementCount(0, COLLADA_ELEMENT_GEOMETRY, 0);
	cout<<"Number of elements "<<numGeo<<"\n";
	String d;
	String d2;

	domGeometry *thisGeometry;
	domMesh *thisMesh;

	if(numGeo == 0)
	{
		cout<<"No Geometry found! Exiting\n";
	}
	
	for(PInt i=0;i<numGeo;++i)
	{		
		error = myDae->getDatabase()->getElement((daeElement**)&thisGeometry, 0, 0, "geometry","export.dae");
		cout<<" Error Code for Geometry "<<i<<" : "<<error<<endl;
		//d = thisGeometry->getName();
		meshName = thisGeometry->getId();
		d = meshName;
		cout<<"Element Name : "<<d<<"\n";

		mesh = IndexedMesh::createIndexedMesh(d + ".sypm",STATIC);
		m = Material::createMaterial(d + ".mat");

		d = thisGeometry->getId();
		cout<<"Element ID : "<<d<<"\n";

		thisMesh = thisGeometry->getMesh();
		num = static_cast<PInt>(thisMesh->getTriangles_array().getCount());
		cout<<"Number of Index Arrays "<<num<<"\n";
		for(PInt l=0;l<num;++l)
		{//For every index array for a material
			domTrianglesRef dt = thisMesh->getTriangles_array().get(l);
			d = dt->getMaterial();
			cout<<"Material Name : "<<d<<"\n";			



			PInt num3 = static_cast<PInt>(dt->getInput_array().getCount());
			numSemantics = num3;
			cout<<"Material Input Semantics count : "<<num3<<"\n";
			for(PInt s=0;s<num3;++s)
			{
				Syp::SubMaterial sm;


				domInputLocalOffsetRef dil = dt->getInput_array().get(s);

				//We get the element being pointed to.
				daeElement* element = 0;

				d = dil->getSemantic();				
				cout<<"Material Input Semantic "<<s<<" : "<<d<<" ";
				d = dil->getSource().getID();
				offset = dil->getOffset();
				cout<<d<<" \n";

				///Determine we have gotten to the source level(lowest level) before continuing
				do
				{	
					myDae->getDatabase()->getElement((daeElement**)&element, 0, d.c_str(), 0);
					d2 = element->getTypeName();
					if(d2 != COLLADA_ELEMENT_SOURCE)
					{
						String type = element->getTypeName();
						cout<<"Non Source type found, attempting to find source for type "<<type<<"\n";
						if(type == COLLADA_ELEMENT_VERTICES)
						{
							domVertices* dv = (domVertices*)(element);
							d = dv->getInput_array().get(0)->getSource().getID();
							cout<<"Found Semantic in child called : "<<dv->getInput_array().get(0)->getSemantic()<<" with source pointing to "<<d<<"\n";
							continue;

						}

					}				
					d = element->getID();
					d2 = element->getTypeName();
					cout<<"Obtained Element : "<<d<<" of type"<<d2<<"\n";
				}while(d2 != COLLADA_ELEMENT_SOURCE);
				////////////////////////////////////////////////////////////////////////////////
				//Now we record down the source array to be associated with the respective semantic
				String semantic = dil->getSemantic();	
				if(semantic == "VERTEX")
				{
					verticesID = d;
					vOffset = offset;
				}
				else if(semantic == "NORMAL")
				{
					normalsID = d;
					nOffset = offset;
				}
				else if(semantic == "TEXCOORD")
				{
					texcoordsID = d;
					tOffset = offset;
				}	


			}

			cout<<"\n\n\n***************************************************************************************\n";
			cout<<"Final sources are as follows:(Damn Collada for being overly complicated)\n";
			cout<<"***************************************************************************************\n";
			cout<<"Vertices are using the array : "<<verticesID<<"\n";
			cout<<"Normals are using the array : "<<normalsID<<"\n";
			cout<<"TexCoords are using the array : "<<texcoordsID<<"\n";
			cout<<"***************************************************************************************\n";

			num3 = static_cast<PInt>(dt->getP()->getValue().getCount());
			cout<<"Material Indices Count : "<<num3<<"\n";

			oldLimits ol;
			ol.indexStart = static_cast<PInt>(indices.size());


			for(PInt m=0;m<num3;++m)
			{
				//cout<<" "<<dt->getP()->getValue().get(m);   
				indices.push_back( dt->getP()->getValue().get(m) );
			}

			ol.indexEnd = static_cast<PInt>(indices.size());

			LimitsArray.push_back(ol);

			///TODO: Do index reordering for vertex cache efficiency here
			cout<<"\n";


		}


		// Reference Arrays ARRAYS-------------------------------------------------------
		num = static_cast<PInt>(thisMesh->getSource_array().getCount());
		cout<<"Number of Source Arrays "<<num<<"\n";
		for(PInt j=0;j<num;++j)
		{

			domSourceRef ds = thisMesh->getSource_array().get(j);
			d = ds->getId();	
			cout<<"SourceArray "<<j<<" ID : "<<d<<"\n";

			FloatArray* filler;
			String semantic = ds->getID();
			if(semantic == verticesID)
			{
				filler = &vertices;
			}
			else if(semantic == normalsID)
			{
				filler = &normals;
			}
			else if(semantic == texcoordsID)
			{
				filler = &texcoords;
			}
			else
			{
				filler = &unknown;
			}
			PInt num2 = ds->getFloat_array()->getCount();
			cout<<"SourceArray "<<j<<" Content Count : "<<num2<<"\n";
			for(PInt k=0;k<num2;++k)
			{
				//cout<<" "<<ds->getFloat_array()->getValue().get(k);
				filler->push_back( static_cast<float>(ds->getFloat_array()->getValue().get(k)) );
			}

			cout<<"\n";

		}

		cout<<"------------------------------------------------------------------\n";
		//Now begins the journey to integrate the data

		processData();
	}







	cout<<"\n Optimised vertices : "<<numOptimized<<"\n";
	cout<<"Num of Unique Vertices : "<<static_cast<PInt>(VertexArray.size())<<"\n";
	mesh->getNumVerticesRef() = VertexArray.size();
	cout<<"Num of Unique Vertices * numSemantics copied in IndexedMesh : "<<static_cast<PInt>(mesh->getDataRef().size())<<"\n";





	if(!mesh->isSkinned())
	{
		PInt offset;
		switch(mesh->getVertType())
		{
		case 1:			
			offset = 8;
			break;
		case 2:			
			offset = 5;
			break;
		case 3:           
			offset = 6;
			break;
		case 4:			
			offset = 6;
			break;
		case 5:			
			offset = 7;
			break;
		case 6:			
			offset = 8;
			break;
		case 7:			
			offset = 9;
			break;
		default:
			assert(false && "WTF ARE YOU LOADING? Invalid vertextype");
			break;
		}

		mesh->getBakDataRef().resize(mesh->getDataRef().size());

		std::copy( mesh->getDataRef().begin(),mesh->getDataRef().end(),mesh->getBakDataRef().begin() );
		for(PInt k=0;k<mesh->getNumVertices();++k)
		{
			//vertices
			mesh->getDataRef()[ k*offset ] = 0;
			mesh->getDataRef()[ k*offset + 1 ] = 0;
			mesh->getDataRef()[ k*offset + 2 ] = 0;
			//normals
			mesh->getDataRef()[ k*offset + 3 ] = 0;
			mesh->getDataRef()[ k*offset + 4 ] = 0;
			mesh->getDataRef()[ k*offset + 5 ] = 0;
		}





		float maty[16];
		float maty2[16];
		Math::IdentityMatrix(maty2);
		//Math::MaxToGLMatrix(maty,maty2);

		for(size_t i=0;i<mesh->getDataRef().size(); i += offset)
		{
			Math::MultVectorByMatrix(
				//Bone [  BoneID from VWeights used by this vertex     ]
				//Use backupdata for original
				//Use the weight for this bone
				//Store in mesh
				maty2,
				//m->getBindPoseMatrix(),
				&mesh->getBakDataRef()[i],
				1.0f,
				&mesh->getDataRef()[i]);

			//Normals
			Math::MultNormalByMatrix(
				maty2,
				//m->getBindPoseMatrix(),
				&mesh->getBakDataRef()[i + 3],
				1.0f,
				&mesh->getDataRef()[i + 3]);
		}


	}

	mesh->setDefaultMaterial("Materials/default.mat");
	mesh->writeToFile(meshName + ".sypm");
	Renderer::getRenderer().updateIndices(mesh);
	Renderer::getRenderer().updateMesh(mesh);
	cout<<"Exporting Completed\n";

	InputManager& im = InputManager::getInputManager();

	do
	{	
		SM.beginDrawing();
		g.run();
		KeyEvent k = im.getKey();
		switch(k.key)
		{
		case SYP_KEY_ESC:
			runGame = false;
			break;	
		}
		SM.endDrawing();
	}while(runGame && g.windowIsOpen());
	delete myDae;
	ResourceManager::getResourceManager().cleanUp();
	SypEngine::releaseSypEngine();
	return 0;
}


//NVTRISTRIP CODE
//#include "NV/NvTriStrip.h"
//#pragma comment(lib, "NvTriStrip.lib")
//SetCacheSize(CACHESIZE_GEFORCE1_2);
//SetListsOnly(true);

//PrimitiveGroup* primGroups = 0;
//PShort numGroups;

//GenerateStrips( &bm->index[0], bm->index.size(),&primGroups, &numGroups);

////version	float
////numTextures 1 byte		upto 256 textures
////numVertices 1 PInt		almost unlim vertices
////numIndexes  1 PShort		65k poly limit
////vertexFormat (VN/VNT/VT) 1 byte	upto 256 formats
////sizePerCompleteVertex 1 byte		each vertex upto 256bytes
////DataSize	  1 PInt				unlim
////Data (in VNT/VN/VT format)		floats
////Indexes		PShorts
//

//
//delete[] primGroups;